#include "LowerAngle.h"

LowerAngle::LowerAngle(double angle, double tolerence ):
PIDCommand("LowerAngle", 1.0, 0, 0, 0.005)
{
//	cout << "LowerAngle::LowerAngle" << endl;
	// Use requires() here to declare subsystem dependencies
	Requires(CommandBase::shooterElevator);
	// eg. requires(chassis);
	m_angle = angle;
	GetPIDController()->SetTolerance(0.02);
	GetPIDController()->SetContinuous(false);
	m_delta = 2;
	m_UpMin = 0.45;
	m_UpMax = 0.65;
	m_DownMin = 0.4;
	m_DownMax = 0.51;
}

double LowerAngle::ReturnPIDInput() {
	double input = CommandBase::shooterElevator->getPotPID();
//	cout << "input: " << input << endl;  
	return input;
}

void LowerAngle::Set(double newangle){
	m_angle = newangle;
}

void  LowerAngle::UsePIDOutput(double output) {
//	cout << "output: " << output << endl;
	
	if(output > 0){
		output = m_UpMin + output * (m_UpMax - m_UpMin);
		output = 0;
	}
	else if (output < 0){
		output = -(m_DownMin + (-output) * (m_DownMax - m_DownMin));
//		output = -0.35;
	}
//	cout << "real output: " << output << endl;
	CommandBase::shooterElevator->elevatorMotor->PIDWrite(-output);
}

// Called just before this Command runs the first time
void LowerAngle::Initialize() {
	// m_angle == 0: we disabled
	if (m_angle) {
	    SetSetpoint(m_angle);
	}
}

// Called repeatedly when this Command is scheduled to run
void LowerAngle::Execute() {
	
}

// Make this return true when this Command no longer needs to run execute()
bool LowerAngle::IsFinished() {
	// m_angle == 0: we disabled
	if (m_angle == 0) {
		return true;
	}
	
	double position = CommandBase::shooterElevator->getPotPID();

	if(m_angle >= position){
		return true;
	}
	return false;
}

// Called once after isFinished returns true
void LowerAngle::End() {
//	cout << "End" << endl;
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void LowerAngle::Interrupted() {
}

